#include "ArmorDetectorT.h"

namespace hnurm
{
    ArmorDetector::ArmorDetector()
    {
        // _kernel=cv::getStructuringElement(cv::MORPH_CROSS,cv::Size(8,8));
    }

    int ArmorDetector::Detect(const ImgInfo& raw_frame,TargetInfo& target)
    {
        _carrier=raw_frame.img;
        _raw_img=_carrier;
        Preprocess();
        CreateBars();
        if(_light_bars.size()>1){
            CreateTarget(_light_bars[0],_light_bars[1],target);}
        ArmorDetector::Reset();
        // int nums=PairBars();
        // ChooseTarget();
        return 1;
    }

    void ArmorDetector::Preprocess()
    {
        // get grey
        cv::cvtColor(_carrier,_carrier,cv::COLOR_BGR2GRAY);
        // binarization processing
        double  aa=cv::threshold(_carrier,_carrier,0,255,cv::THRESH_BINARY|cv::THRESH_OTSU);
//        std::cout<<aa<<std::endl;
//        cv::threshold(_carrier,_carrier,200,255,cv::THRESH_BINARY);

//        cv::imshow("2",_carrier);
    }


    void ArmorDetector::CreateBars()
    {
        cv::Mat _canvas = cv::Mat::zeros(_carrier.size(),CV_8UC1);
        cv::findContours(_carrier,_contours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_SIMPLE);
        for(auto i =0;i<_contours.size();i++)
        {
            for(auto j=0;j<_contours[i].size();j++)
            {
                circle(_canvas,_contours[i][j],0.1,cv::Scalar(255));
            }
        }
        for (int i = 0; i < _contours.size(); i++)
        {
            double area = cv::contourArea(_contours[i]);
            if(area < 10|| area>20000)
                continue;
            if(_contours[i].size()<5)
                continue;
            _tmp_rect= cv::fitEllipse(_contours[i]);
            if(_tmp_rect.size.height/_tmp_rect.size.width>10||area/_tmp_rect.size.area()<0.5||_tmp_rect.size.height/_tmp_rect.size.width<1)
                continue;
            _light_bars.push_back(_tmp_rect);
        }
    }

    void ArmorDetector::CreateTarget(const cv::RotatedRect& rect1,const cv::RotatedRect& rect2,TargetInfo& target)
    {
        cv::Point2f v1[4];
        cv::Point2f v2[4];
        if(rect1.angle>100&&rect2.angle>100)
        {
            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[1].x>v4[1].x)
            {
                rect1.points(v1);
                rect2.points(v2);
            }
            else
            {
                rect1.points(v2);
                rect2.points(v1);
            }
            target.vertexes.push_back(v2[0]);
            target.vertexes.push_back(v1[3]);
            target.vertexes.push_back(v1[2]);
            target.vertexes.push_back(v2[1]);
//            std::cout<<v2[0]<<" "<<v1[3]<<" "<<v1[2]<<" "<<v2[1]<<std::endl;
            target.DrawTarget(_raw_img);
        }
        else if(rect1.angle<80&&rect2.angle<80)
        {
            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[3].x>v4[3].x)
            {
                rect1.points(v1);
                rect2.points(v2);
            }
            else
            {
                rect1.points(v2);
                rect2.points(v1);
            }
            target.vertexes.push_back(v2[2]);
            target.vertexes.push_back(v1[1]);
            target.vertexes.push_back(v1[0]);
            target.vertexes.push_back(v2[3]);
//            std::cout<<v2[2]<<" "<<v1[1]<<" "<<v1[0]<<" "<<v2[3]<<std::endl;
            target.DrawTarget(_raw_img);
        }
        else if(rect1.angle>100&&rect2.angle<80)
        {

            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[1].x>v4[3].x)
            {
                rect1.points(v1);
                rect2.points(v2);
                target.vertexes.push_back(v2[2]);
                target.vertexes.push_back(v1[3]);
                target.vertexes.push_back(v1[2]);
                target.vertexes.push_back(v2[3]);
//                std::cout<<v2[2]<<" "<<v1[3]<<" "<<v1[2]<<" "<<v2[3]<<std::endl;
                target.DrawTarget(_raw_img);
            }
            else if(v3[1].x<v4[3].x)
            {
                rect1.points(v2);
                rect2.points(v1);
                target.vertexes.push_back(v2[0]);
                target.vertexes.push_back(v1[1]);
                target.vertexes.push_back(v1[0]);
                target.vertexes.push_back(v2[1]);
//                std::cout<<v2[0]<<" "<<v1[1]<<" "<<v1[0]<<" "<<v2[1]<<std::endl;
                target.DrawTarget(_raw_img);
            }
        }
        else if(rect1.angle<80&&rect2.angle>100)
        {
            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[1].x>v4[3].x)
            {
                rect1.points(v1);
                rect2.points(v2);
                target.vertexes.push_back(v2[0]);
                target.vertexes.push_back(v1[1]);
                target.vertexes.push_back(v1[0]);
                target.vertexes.push_back(v2[1]);
//                std::cout<<v2[0]<<" "<<v1[1]<<" "<<v1[0]<<" "<<v2[1]<<std::endl;
                target.DrawTarget(_raw_img);
            }
            else if(v3[1].x<v4[3].x)
            {
                rect1.points(v2);
                rect2.points(v1);
                target.vertexes.push_back(v2[2]);
                target.vertexes.push_back(v1[3]);
                target.vertexes.push_back(v1[2]);
                target.vertexes.push_back(v2[3]);
//                std::cout<<v2[2]<<" "<<v1[3]<<" "<<v1[2]<<" "<<v2[3]<<std::endl;
                target.DrawTarget(_raw_img);
            }
        }
        cv::namedWindow("draw_armor_area",0);
        cv::imshow("draw_armor_area",_raw_img);
    }

    void ArmorDetector::Reset()
    {
        _contours.clear();
        _light_bars.clear();
        _armors.clear();
    }

} // namespace hnurm

